#include <iostream>
#include <ros/ros.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/ndt.h>
#include <pcl/registration/gicp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <pclomp/ndt_omp.h>
#include <pclomp/gicp_omp.h>


extern "C"{
    void icp_python(const float (*pc_crop)[4], int pc_crop_n, const float (*pc_car)[4], int pc_car_n, const float *pose_init, float* pose_output);
}

using PointT = pcl::PointXYZ;


// align point clouds and measure processing time
void icp_python(const float (*pc_crop)[4], int pc_crop_n, const float (*pc_car)[4], int pc_car_n, const float *pose_init, float* pose_output) {
  // dlopen("/home/devl/product/lib/python/lib/libpython3.so", RTLD_LAZY | RTLD_GLOBAL);
  pcl::PointCloud<PointT>::Ptr pc_crop_(new pcl::PointCloud<PointT>());
  pcl::PointCloud<PointT>::Ptr pc_car_(new pcl::PointCloud<PointT>());

  PointT point_tmp;
  for(int i = 0; i < pc_crop_n; i++ ){
    point_tmp.x = pc_crop[i][0];
    point_tmp.y = pc_crop[i][1];
    point_tmp.z = pc_crop[i][2];
    pc_crop_->points.push_back(point_tmp);
  }
  for(int i = 0; i < pc_car_n; i++ ){
    point_tmp.x = pc_car[i][0];
    point_tmp.y = pc_car[i][1];
    point_tmp.z = pc_car[i][2];
    pc_car_->points.push_back(point_tmp);
  }
  pc_crop_->width = 1;
  pc_crop_->height = pc_crop_->points.size();
  pc_car_->width = 1;
  pc_car_->height = pc_car_->points.size();

  //debug
  // pcl::io::savePCDFileASCII ("/home/st/ubuntu_data/dataset/gazebo/0509/car/pc_crop.pcd", *pc_crop_);
  // pcl::io::savePCDFileASCII ("/home/st/ubuntu_data/dataset/gazebo/0509/car/pc_car.pcd", *pc_car_);
  // pcl::PCDWriter writer;
  // writer.writeASCII<PointT>("/home/st/ubuntu_data/dataset/gazebo/0509/car/pc_crop.pcd", pc_crop_); //ASCII方式保存
  // writer.writeASCII<PointT>("/home/st/ubuntu_data/dataset/gazebo/0509/car/pc_car.pcd", pc_car_); //ASCII方式保存

  // downsampling
  pcl::PointCloud<PointT>::Ptr downsampled(new pcl::PointCloud<PointT>());

  pcl::VoxelGrid<PointT> voxelgrid;
  double downsample = 0.02;
  voxelgrid.setLeafSize(downsample, downsample, downsample);

  voxelgrid.setInputCloud(pc_crop_);
  voxelgrid.filter(*downsampled);
  *pc_crop_ = *downsampled;

  voxelgrid.setInputCloud(pc_car_);
  voxelgrid.filter(*downsampled);
  *pc_car_ = *downsampled;

  pcl::IterativeClosestPoint<PointT, PointT> icp;
	icp.setMaxCorrespondenceDistance(0.1);
	icp.setTransformationEpsilon(1e-10);
	icp.setEuclideanFitnessEpsilon(0.01);
	icp.setMaximumIterations (100);

	icp.setInputTarget (pc_crop_);
  icp.setInputSource (pc_car_);

  Eigen::Quaternionf q_init(pose_init[3],pose_init[4],
                            pose_init[5],pose_init[6]);
  Eigen::Vector3f pos_init(pose_init[0],
                            pose_init[1],pose_init[2]);
  Eigen::Matrix4f T_matrix = Eigen::Matrix4f::Identity();
  T_matrix.block<3, 3>(0, 0) = q_init.toRotationMatrix();
  T_matrix.block<3, 1>(0, 3) = pos_init;
  pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
  icp.align (*aligned, T_matrix);

  Eigen::Matrix4f trans = icp.getFinalTransformation();

  Eigen::Quaternionf q_mea(trans.block<3, 3>(0, 0));
  pose_output[0] = trans(0, 3);
  pose_output[1] = trans(1, 3);
  pose_output[2] = trans(2, 3);
  pose_output[3] = q_mea.w();
  pose_output[4] = q_mea.x();
  pose_output[5] = q_mea.y();
  pose_output[6] = q_mea.z();

// cout<<pose_output<<endl;
  // visulization
  pcl::visualization::PCLVisualizer vis("vis");
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_handler(pc_crop_, 255.0, 0.0, 0.0);
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_handler(pc_car_, 0.0, 255.0, 0.0);
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_handler(aligned, 0.0, 0.0, 255.0);
  vis.addPointCloud(pc_crop_, target_handler, "target");
  vis.addPointCloud(pc_car_, source_handler, "source");
  vis.addPointCloud(aligned, aligned_handler, "aligned");
  vis.spin();
}
